Solver

Solvers look for state solutions that minimize the error functions of the problem’s Factors.

We designed WOLF to be able to use different solvers. For this, we implement solver wrappers, with a base class SolverManager that can be derived to deal with each different solver library.

The SolverManager class

The SolverManager class is the base class for all solvers in WOLF. It provides a common interface for solving the factor graph encoded in the WOLF tree, managing state blocks, and optionally computing covariances.

It is implemented in the core plugin.

solve()

This is the main method of any solver, it is implemented in SolverManager and includes three stages:

  1. Update the factor graph with the changes in WOLF tree

  2. Call the derived solver via solveDerived()

  3. Update the state blocks in the WOLF tree with the new factor graph estimation

update()

In WOLF, it is the SolverManager the responsible of updating the factor graph problem encoded in the WOLF tree. This is implemented in the update() method.

The changes in the factor graph problem are in two different levels:

  1. Graph changes: New and/or removed factors and/or state blocks.

  2. State changes: Some states may be fixed/unfixed,

    modified its values, and/or added/removed a local parametrization.

These changes are handled in the update() method in two different ways, described below.

1. Graph changes: Problem notifications

In Problem there are two notification maps that store the changes in the factor graph problem.

std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;

These notification maps in Problem, have special getters that also empties the map content. We call it “comsumers”:

void consumeNotifications(std::map<StateBlockPtr, Notification>&, std::map<FactorBasePtr, Notification>&);
std::map<StateBlockPtr, Notification> consumeStateBlockNotificationMap();
std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();

Notification is a simple enum that at the moment can be ADD or REMOVE.

2. State changes: StateBlocks flags

During execution, a processor may update the values of a state block or fix/unfix some others, and a local parameterization can be added or removed. While these changes do not change the factor graph structure, they do change the state of the problem and we want the solver to take them into account when solving the problem. To handle this, StateBlock contains three flags:

std::atomic_bool state_updated_;       ///< Flag to indicate whether the state has been updated
std::atomic_bool fix_updated_;         ///< Flag to indicate whether the status has been updated
std::atomic_bool local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated

They are accessed during the update() execution and apply the changes to the solver.

Partial updates

It may happen that the update() method is called while a processor is running and performing changes in the WOLF tree. In this case, it is likely that a new frame or landmark has been added, but the factors that involves them have not been emplaced yet. This result in an rank-deficient problem, where the solver is not able to compute a solution because the problem is not well defined.

This is not an issue, normally, for most of the solvers. But it is specially problematic if the covariance of the estimated state has to be computed.

To avoid this kind of problems, the update() method handles this “floating” statesç and does not add them to the solver until the factors that involve them are emplaced.

Multi-threading

The SolverManager class, as well as notification machinery in Problem, are designed to be thread-safe. This allows to dedicate an specific thread to the solver, which can run in parallel with the rest of the WOLF system, while still being able to access the problem notifications and state changes.

See also

See the WOLF ROS2 Node implementation, where the solver is run in a separate thread.

Profiling

In SolverManager, there are five profiling units to report the CPU time dedicated to the main processes:

  • Total solving time (includes update factor graph, solve, and update states)

    • Update factor graph

    • Solve

    • Update state blocks

  • Covariance computing time

Deriving from SolverManager

To implement a new solver, you need to derive from the SolverManager class. Analogously to other factory-based nodes in WOLF, the constructor should have a standard API and we provide a macro to automatically implement the creators:

SolverDerived(const ProblemPtr& _wolf_problem, const YAML::Node& _params);
WOLF_SOLVER_CREATE(SolverCeres);

Remember to register the creators in the corresponding factories using the macro (recommended in the .cpp file):

WOLF_REGISTER_SOLVER(SolverCeres)

Finally, you have to override the following pure virtual methods in SolverManager that implements the derived solver:

protected:
    virtual std::string solveDerived(const ReportVerbosity report_level)                     = 0;
    virtual bool        computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
    virtual bool        computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;

    virtual void addFactorDerived(const FactorBasePtr& fac_ptr)                              = 0;
    virtual void removeFactorDerived(const FactorBasePtr& fac_ptr)                           = 0;
    virtual void addStateBlockDerived(const StateBlockPtr& state_ptr)                        = 0;
    virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr)                     = 0;
    virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr)               = 0;
    virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;

    virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const                      = 0;
    virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const                            = 0;
    virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) const                                  = 0;
    virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const                            = 0;
    virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr&               st,
                                                    const LocalParametrizationBasePtr& local_param) const = 0;

    virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0;
    virtual bool checkDerived(std::string prefix = "") const                   = 0;

public:
    virtual bool         converged() const   = 0;
    virtual bool         failed() const      = 0;
    virtual bool         wasStopped() const  = 0;
    virtual unsigned int iterations() const  = 0;
    virtual double       initialCost() const = 0;
    virtual double       finalCost() const   = 0;
    virtual double       totalTime() const   = 0;

SolverCeres

We provide the class SolverCeres in the core plugin, which allows WOLF to be used with Ceres solver.